
#ifndef ROB_MATRIX_H
#define ROB_MATRIX_H

//TODO Cofactor expansion helper functions

#include <cassert>

#include "math.h"

template<size_t n, size_t m, typename T=Scalar>
class Matrix
{
  public:

    // Ctor/Dtor
    Matrix(T data_[n*m]) { for(size_t i=0;i<n*m;++i) data[i]=data_[i]; };
    Matrix() {};

    // Type info
    size_t Rows     () const { return n; }
    size_t Columns  () const { return m; }
    bool   IsValue  () const { return Rows()==1 && Columns()==1; };
    bool   IsQuatric() const { return Rows()==Columns(); }

    // Accessors (by reference)
    T&       Element(size_t row, size_t col)       { assert(row<Rows() && col<Columns()); return data[row*Columns()+col]; }
    const T& Element(size_t row, size_t col) const { assert(row<Rows() && col<Columns()); return data[row*Columns()+col]; }
    // Accessors (by value)
    Matrix<1,m,T> GetRow    (size_t) const;
    Matrix<n,1,T> GetColumn (size_t) const;
    void          SetRow    (size_t,Matrix<1,m,T>);
    void          SetColumn (size_t,Matrix<n,1,T>);

    // Operations (by value)
    // Conversion operator from 1x1 matrix to type T
    T Value() { assert( IsValue() ); return Element(0,0); };
    // Basic arithmetic operations
    Matrix<n,m,T> operator+ (const Matrix<n,m,T>&);
    Matrix<n,m,T> operator- (const Matrix<n,m,T>&);
    Matrix<n,m,T> operator* (Scalar x);
    template<size_t a>
    Matrix<n,a,T> operator* (const Matrix<m,a,T>& rhs);
    // Other operations
    Matrix<m,n,T> Transpose() const;

  protected:
    T data[n*m];

};



// Matrix Addition
template<size_t n, size_t m, typename T>
Matrix<n,m,T> Matrix<n,m,T>::operator+ (const Matrix<n,m,T>& rhs)
{
    Matrix<n,m,T> ret(*this);
    for(size_t row=0;row<ret.Rows();++row)
      for(size_t col=0;col<ret.Columns();++col)
      ret.Element(row,col)+=rhs.Element(row,col);
    return ret;
};
// Matrix Subtraction
template<size_t n, size_t m, typename T>
Matrix<n,m,T> Matrix<n,m,T>::operator- (const Matrix<n,m,T>& rhs)
{
    Matrix<n,m,T> ret(*this);
    for(size_t row=0;row<ret.Rows();++row)
      for(size_t col=0;col<ret.Columns();++col)
      ret.Element(row,col)-=rhs.Element(row,col);
    return ret;
};
// Scalar Multiplication
template<size_t n, size_t m, typename T>
Matrix<n,m,T> Matrix<n,m,T>::operator* (Scalar x)
{
    Matrix<n,m,T> ret(*this);
    for(size_t row=0;row<ret.Rows();++row)
      for(size_t col=0;col<ret.Columns();++col)
        ret.Element(row,col)*=x;
    return ret;
}
// Matrix Multiplication
template<size_t n, size_t m, typename T>
template<size_t a>
Matrix<n,a,T> Matrix<n,m,T>::operator* (const Matrix<m,a,T>& rhs)
{
    Matrix<n,a> ret;
    for(size_t retRow=0;retRow<ret.Rows();++retRow)
      for(size_t retCol=0;retCol<ret.Columns();++retCol)
      {
          ret.Element(retRow,retCol)=T();
          for(size_t i=0; i<m; ++i)
            ret.Element(retRow,retCol)+=Element(retRow,i)*rhs.Element(i,retCol);
      }
    return ret;
}

// Get Row/Column
template<size_t n, size_t m, typename T>
Matrix<1,m,T> Matrix<n,m,T>::GetRow(size_t row) const
{
    Matrix<1,m,T> ret;
    for(size_t col=0;col<Columns();++col)
      ret.Element(0,col)=Element(row,col);
    return ret;
}
template<size_t n, size_t m, typename T>
Matrix<n,1,T> Matrix<n,m,T>::GetColumn(size_t col) const
{
    Matrix<n,1,T> ret;
    for(size_t row=0;row<Rows();++row)
      ret.Element(row,0)=Element(row,col);
    return ret;
}
// Set Row/ColumnMatrix<n,n,T> A
template<size_t n, size_t m, typename T>
void Matrix<n,m,T>::SetRow(size_t row, Matrix<1,m,T> matrix)
{
    for(size_t col=0;col<Columns();++col)
      Element(row,col)=matrix.Element(0,col);
}
template<size_t n, size_t m, typename T>
void Matrix<n,m,T>::SetColumn(size_t col, Matrix<n,1,T> matrix)
{
    for(size_t row=0;row<Columns();++row)
      Element(row,col)=matrix.Element(row,0);
}


template<size_t n, size_t m, typename T>
Matrix<m,n,T> Matrix<n,m,T>::Transpose() const
{
    Matrix<m,n,T> ret;
    for(size_t row=0;row<Rows();++row)
      for(size_t col=0;col<Columns();++col)
        ret.Element(col,row)=Element(row,col);
    return ret;
}



template<size_t n, typename T=Scalar>
class Identity : public Matrix<n,n,T>
{
  public:
    Identity()
    {
        for(size_t row=0;row<n;++row)
          for(size_t col=0;col<n;++col)
            this->Element(row,col)=(row==col)?T(1):T(0);
    }
};



#endif // ROB_MATRIX_H

